//#include <iostream>
//#include <random>
//#include <chrono>
//#include <vector>
//#include "KalmanFiltering.h"
//#include <fstream>
//#include <cmath>
//#include <opencv2/opencv.hpp>
//
//int main() {
//    std::cout << "In" << std::endl;
//
//    //=============保存数据用
//    std::vector<double> X1_True, X2_True;
//
//    KalmanFiltering kf;
//
//    Eigen::MatrixXd p0(4, 4);
//    p0 << 1.0, 0.0, 0.0, 0.0,
//            0.0, 100.0, 0.0, 0.0,
//            0.0, 0.0, 1.0, 0.0,
//            0.0, 0.0, 0.0, 100.0;
//
//    Eigen::MatrixXd inputR(2, 2);
//    inputR << 0.01, 0.0,
//            0.0, 0.01;
//
//    Eigen::MatrixXd inputH(2, 4);
//    inputH << 1.0, 0.0, 0.0, 0.0,
//            0.0, 0.0, 1.0, 0.0;
//
//    kf.Init(p0, inputR, inputH);
//
//
//    cv::Mat frame, resize, hsv, mask;
//    cv::VideoCapture video;
//    cv::Size size(320, 240);
//    video.open("../a.avi");
//
//    double last_timeStamp = 0.0, now_timeStamp;
//
//    while (video.read(frame)) {
//        cv::resize(frame, resize, size, 0, 0, cv::INTER_NEAREST);
//        cv::cvtColor(resize, hsv, cv::COLOR_BGR2HSV);
//        cv::inRange(hsv, cv::Scalar(47, 117, 20), cv::Scalar(77, 255, 255), mask);
//
//        cv::dilate(mask, mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3, 3)));
//        cv::erode(mask, mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3, 3)));
//        cv::dilate(mask, mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3, 3)));
//        cv::erode(mask, mask, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3, 3)));
//        cv::morphologyEx(mask, mask, cv::MORPH_OPEN, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3, 3)));
//        cv::morphologyEx(mask, mask, cv::MORPH_CLOSE, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3, 3)));
//
//        std::vector<std::vector<cv::Point>> contours;
//        findContours(mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE, cv::Point());
//        double area, maxArea = 0;
//        cv::RotatedRect box;
//
//        for (auto &contour: contours) {
//            area = contourArea(contour);
//            if (area < 100) continue;
//            if (area <= maxArea) continue;
//            box = minAreaRect(contour);
//        }
//        cv::Point2f ps1[4];
//        box.points(ps1);
//        cv::Point2f center_MEA = (ps1[0] + ps1[2]) * 0.5;
//        Eigen::MatrixXd inputZ(2, 1);
//        inputZ << (double) center_MEA.x,
//                (double) center_MEA.y;
//
//        now_timeStamp = ((double) cv::getTickCount() * 1000.0 / cv::getTickFrequency()) / 1000.0;
//
//        if (!kf.IsInit()) {
//            last_timeStamp = now_timeStamp;
//            Eigen::VectorXd inputX(4, 1);
//            inputX << (double) center_MEA.x,
//                    2.0,
//                    (double) center_MEA.y,
//                    2.0;
//            kf.InitStateX(inputX);
//        }
//
//        cv::circle(resize, center_MEA, 3, cv::Scalar(255, 255, 255));//绘制检测中心点
////    X1_True.push_back(X_True(0, 0));
////    X2_True.push_back(X_True(1, 0));
//
//        double deltaT = now_timeStamp - last_timeStamp;
//        last_timeStamp = now_timeStamp;
//
//        Eigen::MatrixXd inputA(4, 4);
//        inputA << 1.0, deltaT, 0.0, 0.0,
//                0.0, 1.0, 0.0, 0.0,
//                0.0, 0.0, 1.0, deltaT,
//                0.0, 0.0, 0.0, 1.0;
//        kf.SetA(inputA);
//
//        Eigen::MatrixXd inputQ(4, 4);
////        inputQ << pow(deltaT, 4) / 4.0, 0.0, 0.0, 0.0,
////                0.0, pow(deltaT, 2), 0.0, 0.0,
////                0.0, 0.0, pow(deltaT, 4) / 4.0, 0.0,
////                0.0, 0.0, 0.0, pow(deltaT, 2);
//        inputQ << 1.0, 0.0, 0.0, 0.0,
//                0.0, 1.0, 0.0, 0.0,
//                0.0, 0.0, 1.0, 0.0,
//                0.0, 0.0, 0.0, 1.0;
//        kf.SetQ(inputQ);
//
//        kf.Predict();
//        kf.Correct(inputZ);
//        kf.UpdateP();
//
//        //取得预测结果
//        Eigen::MatrixXd X_Hat = kf.GetX();
//        std::cout << "X_Hat " << X_Hat <<std::endl;
//        cv::Point2f center_Hat((float) X_Hat(0), (float) X_Hat(2));
//        std::cout << "X " << (float) X_Hat(0) <<std::endl;
//        std::cout << "Y " << (float) X_Hat(2) <<std::endl;
//        cv::circle(resize, center_Hat, 3, cv::Scalar(0, 255, 0));//绘制预测中心点
//
//        cv::imshow("resize", resize);
//        cv::imshow("mask", mask);
//
//        if (cv::waitKey(1) == 27)
//            break;
//    }
//
//    //=============保存数据
//    std::ofstream out("../out.txt");
//    auto it = X1_True.begin();
//    if (out.is_open()) {
//        std::cout << "Open" << std::endl;
//        while (it != X1_True.end()) {
//            out << *it << ",";
//            ++it;
//        }
//        out << "\n";
//
//        it = X2_True.begin();
//        while (it != X2_True.end()) {
//            out << *it << ",";
//            ++it;
//        }
//        out << "\n";
//
//        auto X1_P_Hat = kf.GetX1_Prior_Hat();
//        it = X1_P_Hat.begin();
//        while (it != X1_P_Hat.end()) {
//            out << *it << ",";
//            ++it;
//        }
//        out << "\n";
//
//        auto X2_P_Hat = kf.GetX2_Prior_Hat();
//        it = X2_P_Hat.begin();
//        while (it != X2_P_Hat.end()) {
//            out << *it << ",";
//            ++it;
//        }
//        out << "\n";
//
//        auto X1_Hat = kf.GetX1_Hat();
//        it = X1_Hat.begin();
//        while (it != X1_Hat.end()) {
//            out << *it << ",";
//            ++it;
//        }
//        out << "\n";
//
//        auto X2_Hat = kf.GetX2_Hat();
//        it = X2_Hat.begin();
//        while (it != X2_Hat.end()) {
//            out << *it << ",";
//            ++it;
//        }
//        out << "\n";
//
//        out.close();
//    }
//    std::cout << "Out\n" << std::endl;
//    return 0;
//}
//
//// mean:期望， stddev:标准差
//double Gaussian_noise(double mean, double stddev) {
//    std::default_random_engine generator(std::chrono::system_clock::now().time_since_epoch().count());
//    std::normal_distribution<double> dist(mean, stddev);
//    return dist(generator);
//}
//
//cv::Point2f transform(cv::Point2f p, cv::Point2f in, cv::Point2f out) {
//    return cv::Point2f(p.x * out.x / in.x, p.y * out.y / in.y);
//}